/**
 * Author: luoqi
 * Created Date: 2025-08-19 17:10:23
 * Last Modified: 2025-11-26 23:35:24
 * Modified By: luoqi at <**@****>
 * Copyright (c) 2025 <*****>
 * Description:
 */

#ifndef _ROBOMAT_HPP_
#define _ROBOMAT_HPP_

#include <cmath>
#include <Eigen/Dense>

namespace robospace {
    using namespace Eigen;

    template<typename T>
    T deg2rad(T deg)
    {
        return deg * M_PI / 180.0;
    }

    template<typename T>
    T rad2deg(T rad)
    {
        return rad * 180.0 / M_PI;
    }

    template<typename DerivedA, typename DerivedB>
    Matrix<typename DerivedA::Scalar, Dynamic, Dynamic>
        kron(const MatrixBase<DerivedA> &A,
            const MatrixBase<DerivedB> &B)
    {
        using Scalar = typename DerivedA::Scalar;
        int rows = A.rows() * B.rows();
        int cols = A.cols() * B.cols();
        Matrix<Scalar, Dynamic, Dynamic> result(rows, cols);

        for(int i = 0; i < A.rows(); ++i) {
            for(int j = 0; j < A.cols(); ++j) {
                int block_row = i * B.rows();
                int block_col = j * B.cols();
                result.block(block_row, block_col, B.rows(), B.cols()) = A(i, j) * B;
            }
        }
        return result;
    }

    template<typename T>
    Eigen::Vector<T, 3> tform2eul(Eigen::Matrix3<T> R)
    {
        T ry = atan2(-R(2, 0), sqrt(R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0)));
        T rz = atan2(R(1, 0) / cos(ry), R(0, 0) / cos(ry));
        T rx = atan2(R(2, 1) / cos(ry), R(2, 2) / cos(ry));
        return Eigen::Vector<T, 3>(rx, ry, rz);
    }

    template<typename T>
    Eigen::Matrix4<T> vec2tform_deg(Eigen::Vector<T, 6> vec)
    {
        Eigen::Vector3<T> P(vec(0), vec(1), vec(2));
        T rx = deg2rad(vec(3));
        T ry = deg2rad(vec(4));
        T rz = deg2rad(vec(5));
        Eigen::Matrix3<T> R =
            Eigen::AngleAxis<T>(rz, Eigen::Vector3<T>::UnitZ()) *
            Eigen::AngleAxis<T>(ry, Eigen::Vector3<T>::UnitY()) *
            Eigen::AngleAxis<T>(rx, Eigen::Vector3<T>::UnitX()).toRotationMatrix();

        Eigen::Matrix4<T> tform = Eigen::Matrix4<T>::Identity();
        tform.template block<3, 3>(0, 0) = R;
        tform.template block<3, 1>(0, 3) = P;
        return tform;
    }

    template<typename T>
    Eigen::Matrix4<T> vec2tform_rad(Eigen::Vector<T, 6> vec)
    {
        Eigen::Vector3<T> P(vec(0), vec(1), vec(2));
        T rx = vec(3);
        T ry = vec(4);
        T rz = vec(5);
        Eigen::Matrix3<T> R =
            Eigen::AngleAxis<T>(rz, Eigen::Vector3<T>::UnitZ()) *
            Eigen::AngleAxis<T>(ry, Eigen::Vector3<T>::UnitY()) *
            Eigen::AngleAxis<T>(rx, Eigen::Vector3<T>::UnitX()).toRotationMatrix();

        Eigen::Matrix4<T> tform = Eigen::Matrix4<T>::Identity();
        tform.template block<3, 3>(0, 0) = R;
        tform.template block<3, 1>(0, 3) = P;
        return tform;
    }

    template <typename T>
    Eigen::Vector<T, 6> tform2vec_deg(Eigen::Matrix4<T> tform)
    {
        Eigen::Vector<T, 6> vec;
        vec.template block<3, 1>(0, 0) = tform.template block<3, 1>(0, 3);
        Eigen::Matrix3<T> R = tform.template block<3, 3>(0, 0);
        Eigen::Vector3<T> eul = tform2eul(R);
        vec.template block<3, 1>(3, 0) = Eigen::Vector3<T>(rad2deg<T>(eul(0)), rad2deg<T>(eul(1)), rad2deg<T>(eul(2)));
        return vec;
    }

    template <typename T>
    Eigen::Vector<T, 6> tform2vec_rad(Eigen::Matrix4<T> tform)
    {
        Eigen::Vector<T, 6> vec;
        vec.template block<3, 1>(0, 0) = tform.template block<3, 1>(0, 3);
        Eigen::Matrix3<T> R = tform.template block<3, 3>(0, 0);
        Eigen::Vector3<T> eul = tform2eul(R);
        vec.template block<3, 1>(3, 0) = Eigen::Vector3<T>(eul(0), eul(1), eul(2));
        return vec;
    }
}

#endif  // _ROBOMAT_HPP_
